#include <stdlib.h>
#include <stdio.h>
#include <string.h>
//#include <libc.h>
#include "protocol.h"
#include "strtol.h"
#include "hd44780.h"

packets_t *protocol_packets;
unsigned char *protocol_frame;
unsigned char src, dst;

state_t state;

void init_protocol(packets_t *p, unsigned char *frame) {
	protocol_packets = p;
	protocol_packets->num = 0;
	protocol_frame = frame;
	protocol_frame[0] = '\0';
	state = idle;
}

unsigned char get_protocol_data(packets_t *p) {
	if (state == data_ready) {
		state = idle;
		p = protocol_packets;
		return 1;
	}
	else {
		return 0;
	}

}

unsigned char add_to_protocol_decoder(unsigned char s) {
	unsigned char buf[2];
	if (s == ':') {
		// start byte
		protocol_frame[0] = '\0';
		buf[0] = s;
		buf[1] = '\0';
		strcat(protocol_frame, buf);
		state = start_received;
	}
	else if (state == start_received) {
		if (s == '\r') {
			buf[0] = s;
			buf[1] = '\0';
			strcat(protocol_frame, buf);
			state = cr_received;
		}
		else {
			buf[0] = s;
			buf[1] = '\0';
			if ((strlen(protocol_frame) + strlen(buf)) < FRAME_LENGTH) {
				strcat(protocol_frame, buf);
			}
			else {
				// overflow
				state = idle;
				return 0;
			}
		}		
	}
	else if (state == cr_received) {
		if (s == '\n') {
			state = n_received;
			buf[0] = s;
			buf[1] = '\0';
			strcat(protocol_frame, buf);
			// done receiving
			if (decode_frame()) {
				state = data_ready;
			}
			else {
				state = idle;
			}

		}
	}
	return 1;
}

unsigned char add_packet_to_protocol_encoder(packet_t *p) {
	unsigned char hex_buf[2 + 1];
	unsigned char i;
	protocol_packets->packet[protocol_packets->num].cmd = p->cmd;
	for (i = 0; i < PARAMS_LENGTH; i++) {
		protocol_packets->packet[protocol_packets->num].params[i] = p->params[i];
	}
	protocol_packets->num++;

	return 1;
}

unsigned char decode_frame() {
	unsigned char i;
	unsigned char len;
	unsigned char crc[4 + 1];
	unsigned int received_crc_int, crc_int;
	unsigned char hex_buf[4 + 1];
	unsigned char int_buf[3 + 1];
	unsigned char protocol_packet_s[PACKET_LENGTH + 1];

	// get src
	for (i = 0; i < (SRC_SIZE / 2); i++) {
		hex_buf[0] = protocol_frame[START_SIZE + (i * 2)];
		hex_buf[1] = protocol_frame[START_SIZE + (i * 2) + 1];
		hex_buf[2] = '\0';
	}
	src = strtol(hex_buf, NULL, 16);
	sprintf(int_buf, "%d", src);
	crc_int = crc16(int_buf, strlen(int_buf), 0);
	
	// get dst
	for (i = 0; i < (DST_SIZE / 2); i++) {
		hex_buf[0] = protocol_frame[START_SIZE + SRC_SIZE + (i * 2)];
		hex_buf[1] = protocol_frame[START_SIZE + SRC_SIZE + (i * 2) + 1];
		hex_buf[2] = '\0';
	}
	dst = strtol(hex_buf, NULL, 16);
	sprintf(int_buf, "%d", dst);
	crc_int = crc16(int_buf, strlen(int_buf), crc_int);
	
	// get data and decode it
	len = strlen(protocol_frame);
	for (i = 0; i < ((len - (START_SIZE + SRC_SIZE + DST_SIZE + CRC_SIZE + STOP_SIZE)) / 2); i++) {
		hex_buf[0] = protocol_frame[START_SIZE + SRC_SIZE + DST_SIZE + (i * 2)];
		hex_buf[1] = protocol_frame[START_SIZE + SRC_SIZE + DST_SIZE + (i * 2) + 1];
		hex_buf[2] = '\0';

		
		if ((i % PACKET_LENGTH) == 0) {
			protocol_packets->packet[i / PACKET_LENGTH].cmd = strtol(hex_buf, NULL, 16);
			protocol_packets->num++;
		}
		else {
			protocol_packets->packet[i / PACKET_LENGTH].params[(i % PACKET_LENGTH) - CMD_LENGTH] = strtol(hex_buf, NULL, 16);
		}
	}
	for (i = 0; i < protocol_packets->num; i++) {
		memcpy(protocol_packet_s, protocol_packets->packet[i].params, PACKET_LENGTH);
		protocol_packet_s[PACKET_LENGTH] = '\0';
		crc_int = crc16(protocol_packet_s, strlen(protocol_packet_s), crc_int);
	}

	// get crc hex data
	crc[0] = '\0';
	strncat(crc, protocol_frame + (len - (CRC_SIZE + STOP_SIZE)), CRC_SIZE);
	
	// convert it to an int
	len = strlen(crc);
	received_crc_int = 0;
	for (i = 1; i <= len; i++) {
		hex_buf[0] = crc[len - i];
		hex_buf[1] = '\0';
		received_crc_int |= (strtol(hex_buf, NULL, 16)) << (4 * (i - 1));
	}

	// check crc
	if (received_crc_int == crc_int) {	
		return 1;
	}
	else {
		return 0;
	}
}

unsigned char encode_frame(unsigned char src, unsigned char dst) {
	// ':' + src + dst + s (*2) + crc16(4) + '\r' + '\n'
	unsigned char i, j;
	unsigned char crc[4 + 1];
	unsigned int crc_int;
	unsigned char hex_buf[4 + 1];
	unsigned char int_buf[3 + 1];
	unsigned char protocol_packet_s[PACKET_LENGTH + 1];

	// start byte
	protocol_frame[0] = '\0';
	strcpy(protocol_frame, ":");
	
	// src
	sprintf(hex_buf, "%02x", src);
	strcat(protocol_frame, hex_buf);
	sprintf(int_buf, "%d", src);
	crc_int = crc16(int_buf, strlen(int_buf), 0);
	
	// dst
	sprintf(hex_buf, "%02x", dst);
	strcat(protocol_frame, hex_buf);
	sprintf(int_buf, "%d", dst);
	crc_int = crc16(int_buf, strlen(int_buf), crc_int);
	
	// packet
	for (i = 0; i < protocol_packets->num; i++) {
		// cmd
		sprintf(hex_buf, "%02x", protocol_packets->packet[i].cmd);
		strcat(protocol_frame, hex_buf);

		// params
		for (j = 0; j < PARAMS_LENGTH; j++) {
			sprintf(hex_buf, "%02x", protocol_packets->packet[i].params[j]);
			strcat(protocol_frame, hex_buf);
		}
	}

	// crc
	for (i = 0; i < protocol_packets->num; i++) {
		memcpy(protocol_packet_s, protocol_packets->packet[i].params, PACKET_LENGTH);
		protocol_packet_s[PACKET_LENGTH] = '\0';
		crc_int = crc16(protocol_packet_s, strlen(protocol_packet_s), crc_int);
	}
	sprintf(crc, "%02x%02x", (crc_int >> 8), (crc_int & 0xff));
	strcat(protocol_frame, crc);
	
	// stop bytes
	strcat(protocol_frame, "\r");
	strcat(protocol_frame, "\n");
	return 1;
}

